#include "omnidirectional_collision_warning.h"
#include "../remote_vehicle.h"
#include "../utils.h"
#include "../world.h"

#include "interface/msg/event.hpp"

using namespace decision::adas;

OmnidirectionalCollisionWarning::OmnidirectionalCollisionWarning(World& world, double fps)
    : AdasApplication(world, "OCW", fps)
{
}

void OmnidirectionalCollisionWarning::detect()
{
    RCLCPP_INFO(logger_, "OmnidirectionalCollisionWarning do detect");

    auto neighbers = this_world_.get_neighbers();
    if (!neighbers) {
        RCLCPP_INFO(logger_, "OmnidirectionalCollisionWarning No Neighbers!");
        return;
    }

    auto now = muduo::Timestamp::now().microSecondsSinceEpoch();

    for (auto& n : *neighbers) {
        const auto& neighber = n.second;
        auto ttc = neighber->get_ttc();
        auto predicted_min_distance = neighber->get_predicted_min_distance();

        if (std::isnan(ttc) || ttc > 5 || ttc < 0 || predicted_min_distance > 2) {
            continue;
        }

        interface::msg::Event event;

        event.header.stamp.sec = now / 1000 / 1000;
        event.header.stamp.nanosec = (now % 1000) * 1000 * 1000;
        event.header.frame_id = "ocw";

        event.type = "ocw";

        if (ttc < 1) {
            event.level = 0;
        } else if (ttc < 2) {
            event.level = 1;
        } else if (ttc < 5) {
            event.level = 2;
        }

        event.vehicles.emplace_back();
        auto& refv = event.vehicles.back();
        refv.trackid = neighber->trackid();
        refv.x = neighber->x();
        refv.y = neighber->y();
        refv.heading = legalize_angle((M_PI_2 - neighber->heading()) / M_PI * 180);
        refv.width = neighber->width();
        refv.length = neighber->length();
        refv.ttc = ttc;
        refv.azimuth = legalize_angle(-neighber->get_relative_azimuth() / M_PI * 180);
        refv.distance = neighber->get_relative_distance();

        this_world_.report(event);
    }
}
